// #include "g2o/core/sparse_optimizer.h"
// #include "g2o/core/block_solver.h"
// #include "g2o/core/factory.h"
// #include "g2o/core/optimization_algorithm_levenberg.h"
// #include "g2o/solvers/csparse/linear_solver_csparse.h"
 
// #include "g2o/types/slam3d/types_slam3d.h"
 
// #include <iostream>
 
// using namespace std;
// using namespace g2o;
 
#define MAXITERATION 50
#include "../include/Calibrator.h"

using namespace std;
using namespace g2o;

Eigen::Isometry3d Calibrator::setMeasure(int num)
{
    Eigen::Translation3d translation(transfer_vector[num][2],
            transfer_vector[num][3], transfer_vector[num][4]);
    Eigen::AngleAxisd rotation_x(transfer_vector[num][7], Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd rotation_y(transfer_vector[num][6], Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd rotation_z(transfer_vector[num][5], Eigen::Vector3d::UnitZ());

    Eigen::Isometry3d rotationIS = translation * rotation_z * rotation_y * rotation_x;
    return rotationIS;
}

void Calibrator::readCfg()
{
    // read cfg
    string init_file_path="./src/g2o_test/cfg/child_topic_list";
    // in_private_handle.param<std::string>("init_params_file_path", init_file_path, " ");
    ifstream ifs(init_file_path);

    //read sensors 
    ifs>>sensor_num;
    for (int j = 0; j < sensor_num; ++j) {
        string tmp_name;
        ifs>>tmp_name;
        sensor_map.insert(pair<string, int>(tmp_name, j));
    }

    // read transfers
    while(!ifs.eof()) {
        transfer_num++;
        vector<double> tmp_transfer;
        for (int k = 0; k < 2; ++k) {
            // read frames
            string tmp_name;
            ifs>>tmp_name;
            tmp_transfer.push_back(sensor_map[tmp_name]);
        }
        for (int k = 0; k < 6; ++k) {
            // read xyzypr
            double tmp_xyzypr;
            ifs>>tmp_xyzypr;
            tmp_transfer.push_back(tmp_xyzypr);
        }
        transfer_vector.push_back(tmp_transfer);
    }

    for (int i = 0; i < transfer_vector.size(); i++)
    {
        for (int j = 0; j < transfer_vector[i].size(); j++)
        {
            cout<<transfer_vector[i][j]<<" ";
        }
        cout<<endl;
    }
}

void Calibrator::Run()
{
    Calibrator::readCfg();

    // create the linear solver
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();
 
    // create the block solver on the top of the linear solver
    Block* blockSolver = new Block(unique_ptr<Block::LinearSolverType>(linearSolver));
    
    //create the algorithm to carry out the optimization
    OptimizationAlgorithmLevenberg* optimizationAlgorithm = new OptimizationAlgorithmLevenberg(unique_ptr<Block>(blockSolver));
 
/*  //如果没用前面的宏函数，而是调用的是edge_se3和vertex_se3头文件
    //想让程序能识别VertexSE3这些数据类型，就要显示的调用它们，如下
    //如果只用了头文件，而没用显示调用，那么这些数据类型将不会link进来
    //在下面的optimizer.load函数将不能识别这些数据类型
    for(int f=0; f<10;++f)
    {
        VertexSE3* v = new VertexSE3;
        v->setId(f++);
    }
*/
    // create the optimizer
    cout<< "Inintialize optimizer"<<endl;
    SparseOptimizer optimizer;
    optimizer.setAlgorithm(optimizationAlgorithm);
    optimizer.setVerbose(true);


    for ( int i=0; i<sensor_num; i++ )
    {
        cout<< "add vertex" <<endl;
        VertexSE3* v = new VertexSE3();
        v->setEstimate(g2o::SE3Quat(Eigen::Matrix3d::Identity(),Eigen::Vector3d::Zero()));
        v->setId(i);
        vertexSet.push_back(v);
        optimizer.addVertex(v);
    }

    //优化过程中，第一个点固定，不做优化
    VertexSE3* firstRobotPose = dynamic_cast<VertexSE3*>(optimizer.vertex(0));
    firstRobotPose->setFixed(true);

    for ( int i=0; i<transfer_num; i++ )
    {
    cout<< "add edge"<<endl;
    g2o::EdgeSE3* edge = new g2o::EdgeSE3();
    edge->setId(i);
    edge->setVertex ( 0, optimizer.vertex(transfer_vector[i][0]) );
    edge->setVertex ( 1, optimizer.vertex(transfer_vector[i][1]) );
    edge->setMeasurement ( setMeasure(i) ); 
    // edge->setParameterId ( 0,0 );
    edge->setInformation ( Eigen::Matrix<double, 6, 6>::Identity() );
    optimizer.addEdge( edge );
    }

    optimizer.initializeOptimization();
    cerr<<"Optimizing ..."<<endl;
    optimizer.optimize(MAXITERATION);
    cerr<<"done."<<endl;

    for (auto it = sensor_map.begin(); it != sensor_map.end(); ++it)
    {
    Eigen::Isometry3d m=vertexSet[it->second]->estimate();
    cout<<it->first<<": \n"<<m.matrix()<<endl;
    }
}

Calibrator::Calibrator()
{

}